import RPi.GPIO as gpio
import time, sys
from threading import Thread

# 使用gpio 18 16 用来设置pwm 控制左右吧两个风扇电机
pin = 18
pin1 = 16

#  收集轴的操作信号
m1 = 11
m2 = 12

#  压缩轴
m3 = 3
m4 = 5


gpio.setmode(gpio.BOARD)
gpio.setup(pin, gpio.OUT)
gpio.setup(pin1, gpio.OUT)
gpio.setup(m1, gpio.OUT)
gpio.setup(m2, gpio.OUT)
gpio.setup(m3, gpio.OUT)
gpio.setup(m4, gpio.OUT)

# 频率设置为72Hz
p = gpio.PWM(pin, 72)
p.start(0)
p1 = gpio.PWM(pin1, 72)
p1.start(0)

#  分别开多线程对硬件操作进行控制
def my_forwod():
    t = Thread(target=forward,args=(0,))
    t.start()

def my_zou_2():
    t = Thread(target=zhou_2_time, args=())
    t.start()

def my_after():
    t = Thread(target=after, args=(0,))
    t.start()

def my_right():
    t = Thread(target=right, args=(0,))
    t.start()

def my_left():
    t = Thread(target=left, args=(0,))
    t.start()

# 右点击 p 左电机 p1
#  右电机正转  P 18
#  前进
def forward(s):
    a = 1 + s
    while a > 0:
        dc_right = 10.4
        dc_left = 10.4
        p.ChangeDutyCycle(dc_right)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.1)
        dc_right = 10.58
        dc_left = 10.58
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.2)
        p.ChangeDutyCycle(dc_right)
        time.sleep(0.2)
        dc_right = 10.76
        dc_left = 10.76
        p.ChangeDutyCycle(dc_right)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.4)
        dc_right = 10.94
        dc_left  = 10.94
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.1)
        p.ChangeDutyCycle(dc_right)
        time.sleep(0.5)
        a=a-1
        break
    dc_right = 10.4
    dc_left = 10.4
    p.ChangeDutyCycle(dc_right)
    p1.ChangeDutyCycle(dc_left)
    time.sleep(0.1)

#  后退
def after(s):
    a = 1 + s
    while a > 0:
        dc_left = 10.4
        dc_r = 10.4
        p1.ChangeDutyCycle(dc_left)
        p.ChangeDutyCycle(dc_r)
        dc_left = 10.21
        dc_r = 10.21
        p.ChangeDutyCycle(dc_r)
        time.sleep(0.1)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.2)
        dc_left = 10
        dc_r = 10
        p.ChangeDutyCycle(dc_r)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.1)
        dc_left = 9.85
        dc_r = 9.85
        p.ChangeDutyCycle(dc_r)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.2)
        dc_left = 9.75
        dc_r = 9.75
        p.ChangeDutyCycle(dc_r)
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.5)
        a=a-1
        break
    dc_right = 10.4
    dc_left = 10.4
    p.ChangeDutyCycle(dc_right)
    p1.ChangeDutyCycle(dc_left)
    time.sleep(0.1)

#  右转  10 %
def right(s):
    a = 1 + s
    while a > 0:
        dc_left = 10.4
        p1.ChangeDutyCycle(dc_left)
        dc_right = 10.4
        p.ChangeDutyCycle(dc_right)
        time.sleep(0.1)
        dc_left = 10.58
        p1.ChangeDutyCycle(dc_left)
        dc_right = 10.21
        p.ChangeDutyCycle(dc_right)
        time.sleep(0.2)
        dc_left = 10.76
        p1.ChangeDutyCycle(dc_left)
        dc_right = 10
        p.ChangeDutyCycle(dc_right)
        dc_left = 10.95
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.2)
        a = a- 1
        break
    dc_right = 10.4
    dc_left = 10.4
    p.ChangeDutyCycle(dc_right)
    p1.ChangeDutyCycle(dc_left)
    time.sleep(0.1)

#     左转  10%  点一下会动一下
def left(s):
    a = 1 + s
    while a > 0:
        dc_right = 10.4
        p.ChangeDutyCycle(dc_right)
        dc_left = 10.4
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.1)
        dc_right = 10.58
        p.ChangeDutyCycle(dc_right)
        dc_left = 10.21
        p1.ChangeDutyCycle(dc_left)
        time.sleep(0.2)
        dc_right = 10.76
        p.ChangeDutyCycle(dc_right)
        dc_left = 10
        p1.ChangeDutyCycle(dc_left)
        dc_right = 10.95
        p.ChangeDutyCycle(dc_right)
        time.sleep(0.2)
        a = a - 1
        break
    dc_right = 10.4
    dc_left = 10.4
    p.ChangeDutyCycle(dc_right)
    p1.ChangeDutyCycle(dc_left)
    time.sleep(0.1)

def zhou():
    gpio.output(m1, 1)
    gpio.output(m2, 0)

def zhou_stop():
    gpio.output(m1, 0)
    gpio.output(m2, 0)

def zhou_fan():
    gpio.output(m1, 0)
    gpio.output(m2, 1)

def zhou_2():
    gpio.output(m3, 1)
    gpio.output(m4, 0)

def zhou_stop_2():
    gpio.output(m3, 0)
    gpio.output(m4, 0)

def zhou_fan_2():
    gpio.output(m3, 0)
    gpio.output(m4, 1)

def zhou_2_time():
    zhou_2()
    time.sleep(28)
    zhou_stop_2()
    time.sleep(0.5)
    zhou_fan_2()
    time.sleep(22)
    zhou_stop_2()
    forward(0)
    time.sleep(0.5)
    forward(0)
    time.sleep(3)
    zhou_fan_2()
    time.sleep(5.5)
    zhou_stop_2()
